#ifndef CAMERACOLORTRACKER_H
#define CAMERACOLORTRACKER_H

#include <vector>
#include <algorithm>
#include "AxisCamera.h" 
#include "BaeUtilities.h"
#include "FrcError.h"
#include "TrackAPI.h" 
#include "WPILib.h"
class CameraColorTracker {
	TrackingThreshold pink;
	TrackingThreshold green;
	std::vector<ParticleAnalysisReport> targets;
public:
	CameraColorTracker() {
		sprintf (pink.name, "PINK");
		pink.hue.minValue = 220;   
		pink.hue.maxValue = 255;  
		pink.hue.minValue = 150;
		pink.hue.maxValue = 255;
		pink.saturation.minValue = 75;   
		pink.saturation.maxValue = 255;      
		pink.luminance.minValue = 85;  
		pink.luminance.maxValue = 255;
		sprintf (green.name, "GREEN");
		green.hue.minValue = 55;   
		green.hue.maxValue = 125;  
		green.saturation.minValue = 58;   
		green.saturation.maxValue = 255;    
		green.luminance.minValue = 92;  
		green.luminance.maxValue = 255;
	}
	int FindTargets(Image *image, bool greenOnTop) {
		targets.clear();
		std::vector<ParticleAnalysisReport> topParticles;
		std::vector<ParticleAnalysisReport> bottomParticles;
		if (greenOnTop) {
			FindParticles(image, green, &topParticles);
			FindParticles(image, pink, &bottomParticles);
		} else {
			FindParticles(image, pink, &topParticles);
			FindParticles(image, green, &bottomParticles);
		}
		// return bottom particle info
		ParticleAnalysisReport *b, *t;
		for (unsigned int bParticleCounter = 0; bParticleCounter < max(bottomParticles.size(), 10); bParticleCounter++) {
			for (unsigned int tParticleCounter = 0; tParticleCounter < max(topParticles.size(), 10); tParticleCounter++) {
				b = &bottomParticles[bParticleCounter];
				t = &topParticles[tParticleCounter];
				if (b->center_mass_y > t->center_mass_y) {
					// bottom particle is in fact on bottom, so let's check if they are on top of each other
					if (abs(b->center_mass_x - t->center_mass_x)
							< ((b->boundingRect.width + t->boundingRect.width) / 2 )) {
						// they are vertically aligned, awesome, this is a good particle
						targets.push_back(*b);
					}
				}
			}
		}
		return targets.size();
//		if (goodTargets.size() > 0) {
//			return *goodTargets[0];
//		} else {
//			ParticleAnalysisReport fakeReport;
//			fakeReport.particleQuality = 0;
//			return fakeReport;
//		}

	}
	float FindBestTargetX() {
		if (targets.size() > 0) {
			return targets[0].center_mass_x_normalized;
		} else {
			return -2;
		}
	}

	void FindParticles(Image *image, TrackingThreshold threshold, std::vector<ParticleAnalysisReport> *particles) {
		// create an image to put a threshold mask in
		Image *thresholdImage = frcCreateImage(IMAQ_IMAGE_U8);
		imaqSetImageSize(thresholdImage, 320, 240);
		// fill the image: white = match, black = no match
		frcColorThreshold(thresholdImage, image, IMAQ_HSL, 
				&threshold.hue, &threshold.saturation, &threshold.luminance);
		//cout << threshold.hue.minValue;
		
		// figure out how many particles are in the thresholded image
		int particleCount = -1;
		frcCountParticles(thresholdImage, &particleCount);
		cout << "ParticleCount:" << particleCount;
		
		int i;
		ParticleAnalysisReport par; // temporary
		double particleArea; // temporary
		particles->clear();
		// loop through each particle and analyze it
		for (i = 0; i < particleCount; i++) {
			imaqMeasureParticle(thresholdImage, i, 0, IMAQ_MT_AREA, &particleArea);
			// put the particle into the list
			if (particleArea > 50.0) {
				frcParticleAnalysis(thresholdImage, i, &par);
				particles->push_back(par);
				cout << "   Area:" << particleArea << " X:" << par.center_mass_x_normalized;
			}
		}
		cout << "\n";
		// get rid of our threshold mask image
		imaqDispose(thresholdImage);
	}
};
#endif

